#include "stdafx.h"

#include "World.h"
//#include "ParamLoader.h"
#include "Boid.h"
#include "Wall3DStrip.h"
#include "Path.h"


//------------------------------------------------------------------------
//	helper functions
//
//------------------------------------------------------------------------
bool TwoCirclesOverlapped(const OgreVec& v1, OgreReal r1, const OgreVec& v2,OgreReal r2)
{
	OgreReal dist_between_centers = v1.distance(v2);

	if (dist_between_centers < (r1+r2) || dist_between_centers<fabs(r1-r2))
	{
		return true;
	}
	return false;
}

bool World::ObstacleOverlaps(Ogre::Entity* obstacle,OgreReal minGap/* =40::0 */)
{
	const OgreVec &obPos = obstacle->getParentSceneNode()->getPosition();
	const OgreReal obRadius = obstacle->getBoundingRadius();
	
	if(mObstacles.empty()) return false;

	EntityIter obIt = mObstacles.begin();
	while (obIt != mObstacles.end())
	{
		if (TwoCirclesOverlapped(obPos,obRadius, 
			(*obIt)->getParentSceneNode()->getPosition(),(*obIt)->getBoundingRadius()))
		{
			return true;
		}
		obIt++;
	}
	return false;
}

//------------------------------------------------------------------------
//	constructor
//
//------------------------------------------------------------------------
World::World(float dimx,float dimz,Ogre::SceneManager *sm)
	:	mDimX(dimx),
		mDimZ(dimz),
		mSceneManager(sm),
		wbbox(NULL)
{
	/*mDimX=dimx;
	mDimZ=dimz;
	mSceneManager = sm;*/

	float border=3;
	mPath = new Path(5,-(mDimX/2)+border,-(mDimZ/2)+border,
		(mDimX/2)-border,(mDimZ/2)-border,true);

	//
	// setup boids (ignore ini for now, just put one boid in the world)
	
	// start position at the center
	MyVec3 spawnPos(MyVec3::ZERO);

	Boid *pBoid = new Boid("boid1","robot.mesh",mSceneManager,this,
		spawnPos,Prm.BoidScale,MyVec3::ZERO,Prm.MaxSpeed,MyVec3::UNIT_Z,
		Prm.BoidMass,MyRadian(Prm.MaxTurnRatePerSecond),Prm.MaxSteeringForce);

	mBoids.push_back(pBoid);

	CreateWalls();
	CreateObstacles();

}

//------------------------------------------------------------------------
//	destructor
//
//------------------------------------------------------------------------
World::~World()
{
	if (wbbox)
	{
		delete wbbox;
		wbbox=NULL;
	}

	for(unsigned int i=0; i<mBoids.size(); i++)
	{
		delete mBoids[i];
	}
	mBoids.clear();

	/*for (unsigned int i=0; i<mObstacles.size(); i++)
	{
		delete mObstacles[i];
	}*/
	mObstacles.clear();

	delete mPath;
	delete mWalls;
}

//------------------------------------------------------------------------
//	update each boid in the world
//
//------------------------------------------------------------------------
void World::Update(double timeElapsed)
{
	BoidIter bIter = mBoids.begin();
	while (bIter != mBoids.end())
	{
		(*bIter)->Update(timeElapsed);
		bIter++;
	}
}

//------------------------------------------------------------------------
//	create the walls in the world
//
//------------------------------------------------------------------------
void World::CreateWalls()
{
	//Real cornerSize=0.2;
	MyReal halfWidth = mDimX*0.5;
	MyReal halfDepth = mDimZ*0.5;

	//             1------------2
	//            /              \
	//           8                3
	//           |                |
	//           |                |
	//           7                4
	//            \              /
	//             6------------5
	

	MyVec3 walls[8] = {
		MyVec3( -0.8*halfWidth, 0,     -halfDepth),	//1
		MyVec3(  0.8*halfWidth, 0,     -halfDepth),	//2
		MyVec3(      halfWidth, 0, -0.8*halfDepth),	//3
		MyVec3(      halfWidth, 0,  0.8*halfDepth),	//4
		MyVec3(  0.8*halfWidth, 0,      halfDepth),	//5
		MyVec3( -0.8*halfWidth, 0,      halfDepth),	//6
		MyVec3(     -halfWidth, 0,  0.8*halfDepth), //7
		MyVec3(     -halfWidth, 0, -0.8*halfDepth)  //8
	};

	mWalls = new Wall3DStrip(20,8);
	for (unsigned int w=0; w<8; w++)
	{
		mWalls->AddPoint(walls[w]);
	}
	mWalls->AddPoint(walls[0]);	// last point to close the yard
}


//------------------------------------------------------------------------
//	just a slight change......
//
//------------------------------------------------------------------------
void World::CreateObstacles()
{
	MyReal halfWidth = mDimX*0.5;
	MyReal halfDepth = mDimZ*0.5;

	//create a number of randomly positioned barrels
	for (int o=0; o<Prm.NumObstacles; o++)
	{
		Entity* barrel = mSceneManager->
			createEntity("barrel["+Ogre::StringConverter::toString(o)+"]","barrel.mesh");
		mSceneManager->getRootSceneNode()->createChildSceneNode()->attachObject(barrel);
		int radius = barrel->getBoundingRadius();

		const int MinGapBetweenObstacles = 20;
		
		OgreReal randX = Ogre::Math::RangeRandom(-halfWidth+radius, halfWidth-radius);
		OgreReal randZ = Ogre::Math::RangeRandom(-halfDepth+radius, halfDepth-radius);
		barrel->getParentSceneNode()->setPosition(randX,radius*0.5,randZ);

		// continue trying until find a position that doesn't overlaps
		while (ObstacleOverlaps(barrel,MinGapBetweenObstacles))
		{
			randX = Ogre::Math::RangeRandom(-halfWidth+radius, halfWidth-radius);
			randZ = Ogre::Math::RangeRandom(-halfDepth+radius, halfDepth-radius);
			barrel->getParentSceneNode()->setPosition(randX,radius*0.5,randZ);
		}
		
		mObstacles.push_back(barrel);
	}
}


//------------------------------------------------------------------------
//	scale the boids of the world to have the specified new height
//
//------------------------------------------------------------------------
OgreReal World::NormalizeAgents(OgreReal newHeight)
{
	
	// fix the one and only boid for now....
	// TODO apply for many

	BoidIter bIter = mBoids.begin();
	Entity* ent = (*bIter)->getEntity();
	//SceneNode* sn = (*bIter)->getParentSceneNode();

	OgreVec min = ent->getBoundingBox().getMinimum();
	OgreVec max = ent->getBoundingBox().getMaximum();
	OgreReal height = max.y - min.y;
	OgreReal scale = newHeight/height;
	
	Ogre::SceneNode* node = (*bIter)->getParentSceneNode();
	
	node->setScale(OgreVec(scale,scale,scale));
	Ogre::AxisAlignedBox aab = ent->getBoundingBox();
	aab.scale(OgreVec(scale,scale,scale));
	

	// return the previous height of the agents
	return height;
}


//------------------------------------------------------------------------
//	+
//
//------------------------------------------------------------------------
void World::ShowBoidBBox(bool bShow)
{
	if (mBoids.empty())
	{
		return;
	}

	//if(wbbox==NULL)
	//{
	//	BoidIter bIter = mBoids.begin();

	//	//(*bIter)->getEntity()->
	//	wbbox = new WireBoundingBox();
	//	//wbbox->setBoundingBox((*bIter)->getEntity()->getBoundingBox());
	//	wbbox->setupBoundingBox((*bIter)->getEntity()->getBoundingBox());
	//	/*RenderOperation ro;
	//	wbbox->getRenderOperation(ro);
	//	ro.operationType = RenderOperation::OT_LINE_STRIP;
	//	wbbox->setRenderOperation(ro);*/

	//	SceneNode* sn = (*bIter)->getParentSceneNode();
	//	sn->attachObject(wbbox);
	//}
	//
	//wbbox->setVisible(bShow);	

	//mSceneManager->showBoundingBoxes(bShow);



	BoidIter bIter = mBoids.begin();
	(*bIter)->getParentSceneNode()->showBoundingBox(bShow);
}


//------------------------------------------------------------------------
//	+
//
//------------------------------------------------------------------------
void World::ToggleShowBBoxes()
{
	static bool bShow=true;
	mSceneManager->showBoundingBoxes(bShow);
	bShow = !bShow;
}